
//Source file for transformation broadcaster for both the robots

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "tf/transform_datatypes.h"
#include <math.h>
#include <ctime>

std::string robot_name;

  //call back decleration and implementation for pose
  void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  tf::Quaternion q;
  //Quaternion to Roll Pitch and Yaw conversion
  double roll, pitch, yaw;
  tf::Quaternion quat;
  tf::quaternionMsgToTF(msg->pose.pose.orientation, quat);
  tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

  q.setRPY(0.0, 0.0, yaw);
  transform.setRotation(q);

	float corner2 = atan2(msg->pose.pose.position.x-100, 10-msg->pose.pose.position.y);
	float corner3 = atan2(msg->pose.pose.position.y-35, msg->pose.pose.position.x-100);
  	float corner4 = atan2( -5-msg->pose.pose.position.x, msg->pose.pose.position.y-35);
  	float corner1 = atan2(10-msg->pose.pose.position.y, -5-msg->pose.pose.position.x);

	srand((int)time(0));  // 产生随机种子  把0换成NULL也行
	float noise1 = (float)(rand()%100)/100.0-0.5;
  srand((int)time(0));  
  float noise2 = (float)(rand()%75)/75.0-0.35;
  srand((int)time(0));  
  float noise3 = (float)(rand()%85)/85.0-0.425;

if(robot_name=="/master_slave_black"){
    if (msg->pose.pose.position.x  <100 && msg->pose.pose.position.x  > -25  &&  msg->pose.pose.position.y  > -10 &&  msg->pose.pose.position.y  < 10 )
    	transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+noise1, msg->pose.pose.position.y-1+noise1, 0.0) );
	if (msg->pose.pose.position.x  >= 100 && msg->pose.pose.position.x  <= 120  &&  msg->pose.pose.position.y  <= 10 &&  msg->pose.pose.position.y  >=-10 )
		transform.setOrigin( tf::Vector3(msg->pose.pose.position.x +1*sin(corner2), msg->pose.pose.position.y-1*cos(corner2), 0.0) );
        //transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+sin(corner2), msg->pose.pose.position.y-cos(corner2), 0.0) );
    if (msg->pose.pose.position.x  >= 100 && msg->pose.pose.position.x  <= 120  &&  msg->pose.pose.position.y  <= 35 &&  msg->pose.pose.position.y  >=10 )
        transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+1+noise1, msg->pose.pose.position.y+noise1, 0.0) );
	if (msg->pose.pose.position.x  >= 100 && msg->pose.pose.position.x  <= 120  &&  msg->pose.pose.position.y  <= 55 &&  msg->pose.pose.position.y  >=35 )
		transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+1*cos(corner3), msg->pose.pose.position.y+1*sin(corner3), 0.0) );

      if (msg->pose.pose.position.x   > -5 && msg->pose.pose.position.x  <= 100  &&  msg->pose.pose.position.y  < 55 &&  msg->pose.pose.position.y  > 35 )
        transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+noise1, msg->pose.pose.position.y+1+noise1, 0.0) );
	if (msg->pose.pose.position.x  >= -25 && msg->pose.pose.position.x  <= -5  &&  msg->pose.pose.position.y  <= 55 &&  msg->pose.pose.position.y  >=35 )
		transform.setOrigin( tf::Vector3(msg->pose.pose.position.x-1*sin(corner4), msg->pose.pose.position.y+1*cos(corner4), 0.0) );

    if (msg->pose.pose.position.x  >= -25 && msg->pose.pose.position.x  <= -5  &&  msg->pose.pose.position.y  <= 35 &&  msg->pose.pose.position.y  >=10 )
        transform.setOrigin( tf::Vector3(msg->pose.pose.position.x-1+noise1, msg->pose.pose.position.y+noise1, 0.0) );
	if (msg->pose.pose.position.x  >= -25 && msg->pose.pose.position.x  <= -5  &&  msg->pose.pose.position.y  <= 10 &&  msg->pose.pose.position.y  >=-10 )
		transform.setOrigin( tf::Vector3(msg->pose.pose.position.x-1*cos(corner1), msg->pose.pose.position.y+1*sin(corner1), 0.0) );

      //transform.setOrigin( tf::Vector3(msg->pose.pose.position.x-1, msg->pose.pose.position.y-1, 0.0) );
  }

  if(robot_name=="/master_slave_white"){
		if (msg->pose.pose.position.x  <100 && msg->pose.pose.position.x  > -25  &&  msg->pose.pose.position.y  > -10 &&  msg->pose.pose.position.y  < 10 )
        	transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+noise2, msg->pose.pose.position.y+1+noise2, 0.0) );
		if (msg->pose.pose.position.x  >= 100 && msg->pose.pose.position.x  <= 120  &&  msg->pose.pose.position.y  <= 10 &&  msg->pose.pose.position.y  >=-10 )
			transform.setOrigin( tf::Vector3(msg->pose.pose.position.x-1*sin(corner2), msg->pose.pose.position.y+1*cos(corner2), 0.0) );
        	//transform.setOrigin( tf::Vector3(msg->pose.pose.position.x-sin(corner2), msg->pose.pose.position.y+cos(corner2), 0.0) );
      	if (msg->pose.pose.position.x  >= 100 && msg->pose.pose.position.x  <= 120  &&  msg->pose.pose.position.y  <= 35 &&  msg->pose.pose.position.y  >=10 )
        	transform.setOrigin( tf::Vector3(msg->pose.pose.position.x-1+noise2, msg->pose.pose.position.y+noise2, 0.0) );
		if (msg->pose.pose.position.x  >= 100 && msg->pose.pose.position.x  <= 120  &&  msg->pose.pose.position.y  <= 55 &&  msg->pose.pose.position.y  >=35 )
			transform.setOrigin( tf::Vector3(msg->pose.pose.position.x-1*cos(corner3), msg->pose.pose.position.y-1*sin(corner3), 0.0) );

      	if (msg->pose.pose.position.x   > -5 && msg->pose.pose.position.x  <= 100  &&  msg->pose.pose.position.y  < 55 &&  msg->pose.pose.position.y  > 35 )
        	transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+noise2, msg->pose.pose.position.y-1+noise2, 0.0) );
	if (msg->pose.pose.position.x  >= -25 && msg->pose.pose.position.x  <= -5  &&  msg->pose.pose.position.y  <= 55 &&  msg->pose.pose.position.y  >=35 )
		transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+1*sin(corner4), msg->pose.pose.position.y-1*cos(corner4), 0.0) );

     	if (msg->pose.pose.position.x  >= -25 && msg->pose.pose.position.x  <= -5  &&  msg->pose.pose.position.y  <=35 &&  msg->pose.pose.position.y  >=10 )
        	transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+1+noise2, msg->pose.pose.position.y+noise2, 0.0) );
		if (msg->pose.pose.position.x  >= -25 && msg->pose.pose.position.x  <= -5  &&  msg->pose.pose.position.y  <= 10 &&  msg->pose.pose.position.y  >=-10 )
			transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+1*cos(corner1), msg->pose.pose.position.y+1*sin(corner1), 0.0) );
      //transform.setOrigin( tf::Vector3(msg->pose.pose.position.x-1, msg->pose.pose.position.y+1, 0.0) );
  }
  if(robot_name=="/master_slave_tracker"){
      if (msg->pose.pose.position.x  <100 && msg->pose.pose.position.x  > 0  &&  msg->pose.pose.position.y  > -10 &&  msg->pose.pose.position.y  < 10 )
        transform.setOrigin( tf::Vector3(msg->pose.pose.position.x -4+noise3, msg->pose.pose.position.y+noise3, 0.0) );
      else
       transform.setOrigin( tf::Vector3(msg->pose.pose.position.x+noise3, msg->pose.pose.position.y+noise3, 0.0) );
      ROS_INFO("pose Master [x, y]: [%f, %f]", msg->pose.pose.position.x,  msg->pose.pose.position.y);
      /*
      if (msg->pose.pose.position.x  <100 && msg->pose.pose.position.x  > -20  &&  msg->pose.pose.position.y  > -10 &&  msg->pose.pose.position.y  < 10 )
        transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0) );
      if (msg->pose.pose.position.x  >= 100 && msg->pose.pose.position.x  <= 120  &&  msg->pose.pose.position.y  <= 40 &&  msg->pose.pose.position.y  >=-10 )
        transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0) );
      if (msg->pose.pose.position.x   > -10 && msg->pose.pose.position.x  <= 120  &&  msg->pose.pose.position.y  < 50 &&  msg->pose.pose.position.y  >40 )
        transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0) );
      if (msg->pose.pose.position.x  >= -20 && msg->pose.pose.position.x  <= -10  &&  msg->pose.pose.position.y  <= 50 &&  msg->pose.pose.position.y  >=10 )
        transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0) );
        */
  }
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", robot_name));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  if (argc != 2){ROS_ERROR("need robot name as argument"); return -1;};
  robot_name = argv[1];

  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe("/nexus0/base_pose_ground_truth", 10, &poseCallback);

  ros::spin();
  return 0;
};
